/* --------------------------------------------------------------------------
 * GraphomatJr 
 * --------------------------------------------------------------------------
 * prog:  Max Rheiner / Interaction Design / Zhdk / http://iad.zhdk.ch/
 * date:  08/26/2012 (m/d/y)
 * ----------------------------------------------------------------------------
 *
 * ----------------------------------------------------------------------------
 */

import java.lang.Thread;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.ListIterator;
import java.util.ListIterator;

import processing.core.*;
import SimpleDynamixel.*;
import SimpleKDL.*;

public class GraphomatJr extends RoboServoController
{
  static public PVector segmentEnd0 = new PVector(0,0,96);
  static public PVector segmentEnd1 = new PVector(0,0,204);
  static public PVector segmentEnd2 = new PVector(0,25,212.75f);
  static public PVector segmentEnd3 = new PVector(0, 41.8f + 22.4f, 17.8f + 74);

  public GraphomatJr(Object 				parent,	
					 SimpleDynamixel.Servo 	servo)
  {
	super(parent,
		  servo,
		  setupRoboArm(),
		  new int[]{0,1,2,3}, 					// servo id's
		  new float[]{PApplet.radians(180.0f), PApplet.radians(180.0f), PApplet.radians(180.0f+90), PApplet.radians(180.0f)},	// servo offsets in rad
		  100.0f,450.0f		// min - max area for the robo arm
		 );
  }

  protected static SimpleKDL.KinematicSolver setupRoboArm()
  {
	SimpleKDL.Chain chain   = new SimpleKDL.Chain();

	SimpleKDL.Joint    joint;
	SimpleKDL.Frame    frame;
	SimpleKDL.Segment  segment;

	// base z-rot / motor 0
	joint  = new SimpleKDL.Joint(Joint.JointType.RotZ); 
	frame  = new SimpleKDL.Frame(new SimpleKDL.Vector(segmentEnd0.x,segmentEnd0.y,segmentEnd0.z));
	segment = new SimpleKDL.Segment(joint, frame);
	chain.addSegment(segment);

	// x-rot / motor 1
	joint  = new SimpleKDL.Joint(Joint.JointType.RotX); 
	frame  = new SimpleKDL.Frame(new SimpleKDL.Vector(segmentEnd1.x,segmentEnd1.y,segmentEnd1.z));
	segment = new SimpleKDL.Segment(joint, frame);
	chain.addSegment(segment);

	// x-rot / motor 2
	joint  = new SimpleKDL.Joint(Joint.JointType.RotX); 
	frame  = new SimpleKDL.Frame(new SimpleKDL.Vector(segmentEnd2.x,segmentEnd2.y,segmentEnd2.z));
	segment = new SimpleKDL.Segment(joint, frame);
	chain.addSegment(segment);

	// x-rot / motor 3
	joint  = new SimpleKDL.Joint(Joint.JointType.RotX); 
	frame  = new SimpleKDL.Frame(new SimpleKDL.Vector(segmentEnd3.x,segmentEnd3.y,segmentEnd3.z));
//	frame  = new SimpleKDL.Frame(new SimpleKDL.Vector(10 , 38 + 3, 17.8 + 10));
	segment = new SimpleKDL.Segment(joint, frame);
	chain.addSegment(segment);
  
	///////////////////////////////////////////////////////////
	// setup the solver
	float[] minAngles = new float[(int)chain.getNrOfSegments()];
	float[] maxAngles = new float[(int)chain.getNrOfSegments()];
		
	minAngles[0] = PApplet.radians(-179.0f);
	maxAngles[0] = PApplet.radians(179.0f);

	minAngles[1] = PApplet.radians(-22);
	maxAngles[1] = PApplet.radians(105);

	minAngles[2] = PApplet.radians(-20);
	maxAngles[2] = PApplet.radians(170);

	minAngles[3] = PApplet.radians(-115);
	maxAngles[3] = PApplet.radians(180); 

	
	SimpleKDL.KinematicSolver kinematicSolver = new SimpleKDL.KinematicSolver(chain,minAngles,maxAngles);
	// if you only want ik without joint limits
	//kinematicSolver = new SimpleKDL.KinematicSolver(chain);
	
	// set init angles
	SimpleKDL.JntArray initAngles = kinematicSolver.getInitAnglesArray();
	initAngles.set(0,0);
	initAngles.set(1,PApplet.radians(-5));
	initAngles.set(2,PApplet.radians(168));
	initAngles.set(3,0);
/*	// just to see the robot straight
	initAngles.set(1,PApplet.radians(0));
	initAngles.set(2,PApplet.radians(0));
*/

	kinematicSolver.setOutAngles(initAngles);

	return kinematicSolver;
  }


  public int onDrawSegment(KinematicSolver kinematicSolver,PGraphics g,int segmentIndex,PVector endPos)
  {
	
	switch(segmentIndex)
	{
	case 2:
	  // arm from segment 2
	  g.stroke(0,200,200);
	  g.fill(0,200,200,100);

	  g.beginShape();
		g.vertex(0,0,0);
		g.vertex(0,segmentEnd2.y,0);
		g.vertex(0,segmentEnd2.y,segmentEnd2.z);
	  g.endShape();
	  
	  //return KinematicSolver.USER_DRAW_COORSYS | KinematicSolver.USER_DRAW_CONLINE | KinematicSolver.USER_DRAW_ANGLE;
	  break;
	case 3:
	  // arm from segment 3
	  g.stroke(0,200,200);
	  g.fill(0,200,200,100);

	  g.beginShape();
		g.vertex(0,0,0);
		g.vertex(0,segmentEnd3.y,0);
		g.vertex(0,segmentEnd3.y,segmentEnd3.z);
	  g.endShape();
	  
  //    return KinematicSolver.USER_DRAW_COORSYS | KinematicSolver.USER_DRAW_CONLINE| KinematicSolver.USER_DRAW_ANGLE;
	  break;
	default:
	  g.stroke(200,100,200);
	  g.line(0,0,0,endPos.x,endPos.y,endPos.z);
	  break;
	}
	
	return KinematicSolver.USER_DRAW_ALL;
  }

}